05. Global Kinematic Model

Now we’ve developed equations to determine the next state (state at t+1) from our state vector at t and our actuator values. Note that we’ve added a variable to our state called L_f which measures the distance between the front of the vehicle and its center of gravity. The larger the vehicle , the slower the turn rate.

\large x_{t+1} = x_t + v_t cos(\psi_t) * dt

\large y_{t+1} = y_t + v_t sin(\psi_t) * dt

\large \psi_{t+1} = \psi_t + \frac {v_t} { L_f} \delta * dt

\large v_{t+1} = v_t + a_t * dt

In the quiz below we will implement the global kinematic model to return the next state vector, given a current state input vector and an actuator vector.

Start Quiz:

// In this quiz you'll implement the global kinematic model.
#include <math.h>
#include <iostream>
#include "Dense"

using Eigen::VectorXd;

//
// Helper functions
//
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

const double Lf = 2;

// Return the next state.
VectorXd globalKinematic(const VectorXd &state, 
                         const VectorXd &actuators, double dt);

int main() {
  // [x, y, psi, v]
  VectorXd state(4);
  // [delta, v]
  VectorXd actuators(2);

  state << 0, 0, deg2rad(45), 1;
  actuators << deg2rad(5), 1;

  // should be [0.212132, 0.212132, 0.798488, 1.3]
  auto next_state = globalKinematic(state, actuators, 0.3);

  std::cout << next_state << std::endl;
}

VectorXd globalKinematic(const VectorXd &state, 
                         const VectorXd &actuators, double dt) {
  // Create a new vector for the next state.
  VectorXd next_state(state.size());

  /**
   * TODO: Implement the global kinematic model,
   *   to return the next state from the inputs.
   */

  // NOTE: state is [x, y, psi, v] and actuators is [delta, a]

  return next_state;
}

[x, y, \psi, v] is the state of the vehicle, L_f is a physical characteristic of the vehicle, and [\delta, a] are the actuators, or control inputs, to our system.

This is a good kinematic model!

Now that we have this, we can use the model to write a simulation where we can develop a controller to follow a trajectory.